#ifndef DOG_MOTION_HPP
#define DOG_MOTION_HPP

#include <ros/ros.h>
#include <iostream>
#include <thread>
#include <chrono>
#include <cstring>
#include <vector>
#include <map>
#include <cmath>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstdint>

// 机器人状态上传结构体
struct RobotStateUpload {
    int robot_basic_state; //机器人基本运动状态
    int robot_gait_state; //机器人当前步态
    double rpy[3]; //机器人roll, pitch, yaw姿态 IMU角度信息
    double rpy_vel[3]; //机器人roll, pitch, yaw角速度
    double xyz_acc[3]; //机器人x, y, z加速度
    double pos_world[3]; //机器人世界坐标系下的位置
    double vel_world[3]; //机器人世界坐标系下的速度
    double vel_body[3]; //机器人机体坐标系下的速度
    unsigned touch_down_and_stair_trot; //无效 仅占位
    bool is_charging;; //无效 仅占位
    unsigned error_state; //无效 仅占位
    int robot_motion_state; //机器人运动状态
    double battery_level; //机器人电池电量
    int task_state; //无效 仅占位
    bool is_robot_need_move; //机器人是否需要移动
    bool zero_position_flag; //机器人是否需要归零
    double ultrasound[2]; //超声波传感器数据
};

// 心跳包发送类
class HeartBeat {
private:
    int udp_socket;
    struct sockaddr_in send_addr;
    bool running;
    std::thread heart_thread;

    void send_heartbeat();

public:
    HeartBeat(const char* ip = "192.168.1.120", int port = 43893);
    ~HeartBeat();
};

// 状态监控类
class StateMonitor {
private:
    int udp_socket;
    struct sockaddr_in recv_addr;
    bool running;
    std::thread monitor_thread;
    RobotStateUpload state;

    void receive_state();
    void parse_state_data(char* data, int length);
    void update_derived_state();

public:
    StateMonitor(const char* ip = "192.168.1.120", int port = 43894);
    ~StateMonitor();

    const RobotStateUpload& get_state() const { return state; }

    // 状态检查方法
    std::pair<bool, std::string> check_power() const;
    std::pair<bool, std::string> check_battery(int min_level = 10) const;
    std::pair<bool, std::string> check_standing() const;
    std::pair<bool, std::string> check_mode(const std::string& required_mode) const;
    std::pair<bool, std::string> check_posture(float max_angle = 30.0f) const;
    std::pair<bool, std::string> check_motion(const std::string& action_name) const;
};

// 机器人控制类
class RobotController {
private:
    int udp_socket;
    struct sockaddr_in send_addr;
    HeartBeat heartbeat;
    StateMonitor state_monitor;
    ros::NodeHandle nh;

    bool send_command(int code, int value, int type);

public:
    RobotController(const char* ip = "192.168.1.120", int port = 43893);
    ~RobotController();

    // 基础控制命令
    bool stand_up_or_lie_down();
    bool soft_emergency_stop();
    bool zero_position();

    // 姿态调整命令（原地模式）
    bool adjust_roll(int value);
    bool adjust_pitch(int value);
    bool adjust_height(int value);
    bool adjust_yaw(int value);

    // 移动控制命令
    bool move_side(int value);
    bool move_forward(int value);
    bool turn(int value);

    // 模式切换命令
    bool set_spot_mode();
    bool set_move_mode();

    // 步态控制命令
    bool set_flat_low_speed_gait();
    bool set_flat_medium_speed_gait();
    bool set_flat_high_speed_gait();

    // 特殊动作命令
    bool twist_body();
    bool backflip();
    bool space_step();
    bool greet();

    // 系统控制命令
    bool autonomous_mode();
    bool manual_mode();

    // AI功能控制
    bool close_all_ai_options();
    bool enable_obstacle_stop();
    bool enable_follow();
};

#endif // DOG_MOTION_HPP 